#ifndef JOINT_H
#define JOINT_H

#include <array>
#include <cmath>
#include <glm/glm.hpp>
#include <glm/mat4x4.hpp>

class XB4
{
private:
    std::array<float, 7> a{};
    std::array<float, 7> alpha{};
    std::array<float, 7> d{};
    std::array<float, 7> theta{};

    uint32_t wDirty = 0;
    std::array<glm::mat4, 7> wTrans{};
    std::array<glm::mat4, 7> lTrans{};
    std::array<bool, 7> lDirty{};

public:
    XB4()
    {
        a[0] = 0; alpha[0] = 0; d[0] = 0; theta[0] = 0;

        a[1] = 0; alpha[1] = 0; d[1] = 342; theta[1] = 0;

        a[2] = 40; alpha[2] = glm::radians(-90.0f); d[2] = 0; theta[2] = glm::radians(-90.0f);

        a[3] = 275; alpha[3] = 0; d[3] = 0; theta[3] = 0;

        a[4] = 25; alpha[4] = glm::radians(-90.0f); d[4] = 280; theta[4] = 0;

        a[5] = 0; alpha[5] = glm::radians(90.0f); d[5] = 0; theta[5] = 0;

        a[6] = 0; alpha[6] = glm::radians(-90.0f); d[6] = 0; theta[6] = 0;

        wDirty = 0;
        for (int i = 0; i < 7; ++i) {
            lDirty[i] = true;
        }
    }

    void SetJointPara(uint32_t index, float para)
    {
        if (theta[index] == glm::radians(para))
            return;
        theta[index] = glm::radians(para);
        if (wDirty > index)
            wDirty = index;
        lDirty[index] = true;
    }

    float GetJointPara(uint32_t index)
    {
        return glm::degrees(theta[index]);
    }

    glm::mat4 GetJointTransform(uint32_t index)
    {
        if(index < wDirty)
            return wTrans[index];

        auto myT = glm::mat4(1.0f);

        if (lDirty[index]) {
            myT = glm::mat4(
                    1.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, 1.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, d[index], 1.0f
            ) * myT;

            auto theta_i = theta[index];
            myT = glm::mat4(
                    cosf(theta_i), sinf(theta_i), 0.0f, 0.0f,
                    -sinf(theta_i), cos(theta_i), 0.0f, 0.0f,
                    0.0f, 0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 0.0f, 1.0f
            ) * myT;

            myT = glm::mat4(
                    1.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, 1.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 1.0f, 0.0f,
                    a[index], 0.0f, 0.0f, 1.0f
            ) * myT;

            auto alpha_i = alpha[index];
            myT = glm::mat4(
                    1.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, cosf(alpha_i), sinf(alpha_i), 0.0f,
                    0.0f, -sinf(alpha_i), cosf(alpha_i), 0.0f,
                    0.0f, 0.0f, 0.0f, 1.0f
            ) * myT;
            lTrans[index] = myT;
            lDirty[index] = false;
        } else
        {
            myT = lTrans[index];
        }

        if(index != 0) {
            auto parentT = GetJointTransform(index - 1);
            myT = parentT * myT;
        }

        wTrans[index] = myT;
        wDirty = index + 1;
        return myT;
    }
};

#endif //JOINT_H
